#include <msp430x16x.h>

#include "../timing/timing.h"
#include "../adc/adc.h"
#include "adcs.h"

/*
  ADCS() Selects XYZ channels for ADCS subsystem and samples the output using the ADC
*/

void ADCS(int *attitude)
{
  char channelSequence[4] = {16, 0, 4, 0}; /* x (00010000) y (00000000) z (00000100) y (00000000) */
  int outputIndex=0;

  /* Set control pins P5.5 and P5.3 to output (000X0X00) Won't be needed once all input/outputs are allocated */
  P5DIR|=0x24;

  //Temporary; resets ADCS with P5.7 each time 4 readings are taken. This is for testing and should be re-evaluated later on
  P5DIR|=0x40;
  P5OUT|=0x40;
  waitms(100);
  P5OUT&=0x40;

  /* Select ADCS channels using port 2 and store samples from ADC to attitude array */
  for(outputIndex=0; outputIndex<4; outputIndex++)
  {
    P5OUT=channelSequence[outputIndex];
    WAITUS(641);
    if (outputIndex<3) attitude[outputIndex]=adcSample(7,1);
  }

  return;
}
